//
// File: singleKalman.cpp
//
// MATLAB Coder version            : 5.4
// C/C++ source code generated on  : 04-May-2024 23:17:53
//

// Include Files
#include "singleKalman.h"
#include "singleKalman_data.h"
#include "singleKalman_initialize.h"

// Variable Definitions
static double x0[2];

static double P0[4];

// Function Definitions
//
// 滤波系数
//
// Arguments    : double rawDis
// Return Type  : double
//
double singleKalman(double rawDis)
{
  static const double Q[4]{0.01, 0.0, 0.0, 0.01};
  static const signed char c_b[4]{1, 0, 0, 1};
  double P_pred[4];
  double b[4];
  double x_pred[2];
  double K_idx_0;
  double K_idx_1;
  double b_b;
  double d;
  double d1;
  double d2;
  if (!isInitialized_singleKalman) {
    singleKalman_initialize();
  }
  // end
  //  初始化静态变量
  // step_end
  // 测量值导入measurement
  // end
  //  预测下一个状态
  K_idx_1 = x0[0];
  b_b = x0[1];
  K_idx_0 = P0[0];
  d = P0[1];
  d1 = P0[2];
  d2 = P0[3];
  for (int i{0}; i < 2; i++) {
    double d3;
    double d4;
    int i1;
    int x_pred_tmp;
    x_pred_tmp = c_b[i];
    i1 = c_b[i + 2];
    d3 =
        static_cast<double>(x_pred_tmp) * K_idx_0 + static_cast<double>(i1) * d;
    x_pred[i] = static_cast<double>(x_pred_tmp) * K_idx_1 +
                static_cast<double>(i1) * b_b;
    d4 = static_cast<double>(x_pred_tmp) * d1 + static_cast<double>(i1) * d2;
    P_pred[i] = (d3 + d4 * 0.0) + Q[i];
    P_pred[i + 2] = (d3 * 0.0 + d4) + Q[i + 2];
  }
  //  预测误差协方差
  //  更新步骤
  K_idx_1 = (P_pred[0] + 0.0 * P_pred[1]) + (P_pred[2] + 0.0 * P_pred[3]) * 0.0;
  //  计算卡尔曼增益
  K_idx_0 = (P_pred[0] + P_pred[2] * 0.0) / (K_idx_1 + 0.01);
  K_idx_1 = (P_pred[1] + P_pred[3] * 0.0) / (K_idx_1 + 0.01);
  b_b = rawDis - (x_pred[0] + 0.0 * x_pred[1]);
  x0[0] = x_pred[0] + K_idx_0 * b_b;
  x0[1] = x_pred[1] + K_idx_1 * b_b;
  //  更新状态估计
  b[0] = 1.0 - K_idx_0;
  b[1] = 0.0 - K_idx_1;
  b[2] = 0.0 - K_idx_0 * 0.0;
  b[3] = 1.0 - K_idx_1 * 0.0;
  K_idx_1 = P_pred[0];
  b_b = P_pred[1];
  K_idx_0 = P_pred[2];
  d = P_pred[3];
  for (int i{0}; i < 2; i++) {
    d1 = b[i + 2];
    d2 = b[i];
    P0[i] = d2 * K_idx_1 + d1 * b_b;
    P0[i + 2] = d2 * K_idx_0 + d1 * d;
  }
  //  更新误差协方差
  return x0[0];
}

//
// 滤波系数
//
// Arguments    : void
// Return Type  : void
//
void singleKalman_init()
{
  x0[0] = 0.0;
  x0[1] = 0.0;
  P0[1] = 0.0;
  P0[2] = 0.0;
  P0[0] = 1.0;
  P0[3] = 1.0;
}

//
// File trailer for singleKalman.cpp
//
// [EOF]
//
